#include <math.h>
#include "MatUtil.h"
#include "mat3.h"

float MatUtil::DEG_TO_RAD = (float) M_PI / 180;

Mat4 MatUtil::rotateX(float theta)
{
	Mat4 m;
	float c = cosf(DEG_TO_RAD * theta);
	float s = sinf(DEG_TO_RAD * theta);

	m[1][1] = c;
	m[1][2] = -s;
	m[2][1] = s;
	m[2][2] = c;

	return m;
}

Mat4 MatUtil::rotateY(float theta)
{
	Mat4 m;
	float c = cosf(DEG_TO_RAD * theta);
	float s = sinf(DEG_TO_RAD * theta);

	m[0][0] = c;
	m[0][2] = -s;
	m[2][0] = s;
	m[2][2] = c;

	return m;
}

Mat4 MatUtil::rotateZ(float theta)
{
	Mat4 m;
	float c = cosf(DEG_TO_RAD * theta);
	float s = sinf(DEG_TO_RAD * theta);

	m[0][0] = c;
	m[0][1] = -s;
	m[1][0] = s;
	m[1][1] = c;

	return m;
}

Mat4 MatUtil::rotate(float tx, float ty, float tz)
{
	Mat4 R;

	if (tx != 0) {
		R *= rotateX(tx);
	}

	if (ty != 0) {
		R *= rotateY(ty);
	}

	if (tz != 0) {
		R *= rotateZ(tz);
	}

	return R;
}

Mat4 MatUtil::rotateArbitraryAxis(const Vec3& u, float theta)
{
	float c = cosf(DEG_TO_RAD * theta);
	float s = sinf(DEG_TO_RAD * theta);

	Mat4 uxu = Mat4(
		Vec4(u.x*u.x, u.x*u.y, u.x*u.z, 0.0f),
		Vec4(u.x*u.y, u.y*u.y, u.y*u.z, 0.0f),
		Vec4(u.x*u.z, u.y*u.z, u.z*u.z, 0.0f),
		Vec4(0.0f, 0.0f, 0.0f, 0.0f)
	);

	Mat4 ux = Mat4(
		Vec4(0.0f, -u.z, -u.y, 0.0f),
		Vec4(u.z, 0.0f, -u.x, 0.0f),
		Vec4(u.y, u.x, 0.0f, 0.0f),
		Vec4(0.0f, 0.0f, 0.0f, 0.0f)
	);

	return Mat4(c) + s*ux + (1-c)*uxu;
}

Mat4 MatUtil::translate(float x, float y, float z)
{
	Mat4 c;

	c[0][3] = x;
	c[1][3] = y;
	c[2][3] = z;

	return c;
}

Mat4 MatUtil::scale(float x, float y, float z)
{
	Mat4 m;

	m[0][0] = x;
	m[1][1] = y;
	m[2][2] = z;

	return m;
}

Mat4 MatUtil::inverseTranlations(const Mat4& m)
{
	Mat4 mi = m;
	
	mi[0][3] = -mi[0][3];
	mi[1][3] = -mi[1][3];
	mi[2][3] = -mi[2][3];

	return mi;
}

Mat4 MatUtil::inverseRotations(const Mat4& m)
{
	return m.transpose();
}